#ifndef PATHSROUTING_H 
#define PATHSROUTING_H

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <vector>
#include <queue>
#include <utility>
#include <iostream>
#include <chrono>
#include <memory>
#include <glog/logging.h>
#include "std_msgs/String.h"
#include <string>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/Marker.h>
#include <tf2/utils.h>
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <unordered_map>
#include "match_projection_point.h"
#include "PathManager.h"
#include "ObstaclesManager.h"
#include "Obstacles.h"
#include "paths_routing_parameters.h"
#include "cached_path_profile.h"
#include "paths_struct.h"
#include "FrenetPath.h"
#include "FrenetOptimalTrajectory.h"
#include "cartesian_to_frenet.h"
#include "CubicSpline2D.h"

using std::string;

class Paths_Routing
{
public:

    Paths_Routing();
    ~Paths_Routing();
    void paths_routing_thread(void);
    void global_paths(const nav_msgs::Path &path_point_);
    void Odom(const nav_msgs::Odometry &odom_msg_);
    void laserCallback(const sensor_msgs::LaserScan& scan);
    void GoalPoseCallback(const geometry_msgs::PoseStamped &msg);
    bool paths_segment_intercept(int proj_index, CachedPathProfile &cache);
    void vehicle_outline();
    bool is_collision(FrenetPath *fts, double COLLISION_CHECK);
    void best_local_path(FrenetPath *best_frenet_path);
    bool is_update_dynamic(nav_msgs::Path &trj_point_array, nav_msgs::Odometry &odom, int size);
    void best_path_information(FrenetPath* best_frenet_path);

private:

    double d0;                   // 初始的横向偏移值 [m]
    double dd0;                  // 初始的横向速度 [m/s]
    double ddd0;                 // 初始的横向加速度 [m/s^2]
    double ds0;                  // 初始的纵向速度[m/s]
    double dds0;                 // 初始的纵向加速度[m/ss]
    double init_lon_state;
    double theta_end;
    double best_ds0;

    int num_next;

    bool is_match_projection = false;
    bool is_path_segment = false;
    bool is_global_s_heading_ = true;
    bool is_global_s_heading_success = false;
    bool is_frist_odom = false;
    bool messege2;
    bool local_finish = false;
    bool is_goal_receive = false;
    bool messege1;

    // std::vector<PointAndIndex> obstacles_projections;
    std::vector<std::pair<double, double>> global_path_points;
    std::vector<std::pair<double, double>> s_d_point;
    std_msgs::String start_dynamic;  // 判断局部规划是否开始

    nav_msgs::Odometry odom;
    geometry_msgs::Pose goal_pose_;
    nav_msgs::Path traj_points_;

    geometry_msgs::PoseArray pubLocalPath_a;
    geometry_msgs::PoseArray pubLocalPath_t;
    geometry_msgs::PoseArray pubLocalPath_k;
    geometry_msgs::PoseArray pubLocalPath_v;


    ros::NodeHandle nh;
    ros::Subscriber global_paths_receive_;
    ros::Subscriber odom_;
    ros::Subscriber laser_;
    ros::Subscriber GoalPose_;

    ros::Publisher vehicle_outline_pub_;
    ros::Publisher local_path_pub_;
    ros::Publisher local_paths_k;
    ros::Publisher local_paths_t;
    ros::Publisher local_paths_a;
    ros::Publisher local_paths_v;
    ros::Publisher Start_Dynamic_pub_;

    Match_Projection_Point match_pp;
    PointAndIndex projection_point;
    // PathManager path_manager;
    ObstaclesManager obs_manager;
    Obstacles obs;
    Paths_Routing_Parameters paths_rou_par;
    CachedPathProfile cache;
    PathsSegmentCached  PathSegment;
    CartesianToFrenet cft;
    Point point;
    FrenetPath *best_frenet_path;    // 最佳路径,要析构

    boost::thread *paths_routing_thread_;
    FrenetOptimalTrajectory frenet_opt_tra;

    std::unique_ptr<CubicSpline2D> Cub_spline_2D;
};

#endif // PATHSROUTING_H
